/*
 * MatrixOperation.cpp
 *
 *  Created on: 10.11.2009
 *  Author: 	Martin Do
 *	Copyright: 	Martin Do, Chair Prof. Dillmann (IAIM),
 *            	Institute for Anthropomatics (IFA),
 *		        Karlsruhe Institute of Technology (KIT). All rights reserved.
 *
 *	Changes:
 *
 *	Description:
 */



#include "MatrixOperation.h"

matrix CMatrixOperation::getMatrixFromData(std::vector<gsl::vector> data)
{
    matrix dataMatrix = matrix (data[0].size(), data.size());
    for (unsigned int i = 0; i < data.size(); i++)
        for (unsigned int j = 0; j < data[0].size(); j++) {
			//printf("~~~ %d %d %d %d\n", i, j, data.size(), data[0].size());
            dataMatrix(j, i) = data[i](j);
		}
		      
    return dataMatrix;
}

std::vector<gsl::vector> CMatrixOperation::getDataFromMatrix(matrix dataMatrix)
{
  std::vector<gsl::vector> dataSet;
  for (unsigned int i = 0; i < dataMatrix.get_cols(); i++)
    {
      gsl::vector dataEntry(dataMatrix.get_rows());
      for (unsigned int j = 0; j < dataMatrix.get_rows(); j++)
	dataEntry(j) = dataMatrix(j,i);
      dataSet.push_back(dataEntry);
    }
  return dataSet;
}

gsl::vector CMatrixOperation::getMeanVector(matrix dataMatrix)
{
	//mean of all dim
	gsl::vector means(dataMatrix.get_rows());
	means.set_zero();
    for (unsigned int i = 0; i < dataMatrix.get_rows(); i++)
	{
		means(i) = dataMatrix.row(i).sum();
		means(i) /= dataMatrix.get_cols();
	}
	return means;
}

matrix CMatrixOperation::getCovarianceMatrix(matrix dataMatrix, gsl::vector means)
{
	int nDataInstances = dataMatrix.get_cols();
	int dataDim = dataMatrix.get_rows();
	matrix covMatrix(dataDim,dataDim);
	covMatrix.set_zero();
	for (int i = 0; i < nDataInstances; i++)
		{
		matrix tempCovMatrix(dataDim, dataDim);
		tempCovMatrix.set_zero();
		for (int j = 0; j < dataDim; j++)
			for (int k = 0; k < dataDim; k++)
				tempCovMatrix(j,k) = (dataMatrix(j,i)-means(j))*(dataMatrix(k,i)-means(k));
		covMatrix += tempCovMatrix;
		}
	covMatrix = covMatrix/nDataInstances;
	return covMatrix;
}

matrix CMatrixOperation::getCorrelationMatrix(matrix dataMatrix, gsl::vector means)
{
	int nDataInstances = dataMatrix.get_cols();
    int dataDim = dataMatrix.get_rows();
	matrix covMatrix(dataDim,dataDim);
	covMatrix.set_zero();
	for (int i = 0; i < nDataInstances; i++)
		{
		matrix tempCovMatrix(dataDim, dataDim);
		tempCovMatrix.set_zero();
		for (int j = 0; j < dataDim; j++)
			for (int k = 0; k < dataDim; k++)
				tempCovMatrix(j,k) = (dataMatrix(j,i)-means(j))*(dataMatrix(k,i)-means(k));
		covMatrix += tempCovMatrix;
		}

	matrix stdDerivation(dataDim, dataDim);
	stdDerivation.set_zero();
    double* stdDerivations = new double[dataDim];
    for (int j = 0; j < dataDim; j++)
	{
		stdDerivations[j] = 0.0;
		for (int i = 0; i < nDataInstances; i++)
			stdDerivations[j] += (dataMatrix(j,i)-means(j))*(dataMatrix(j,i)-means(j));
		printf("stdDeriv %e\n",stdDerivations[j]);
	}

	for (int j = 0; j < dataDim; j++)
		for (int k = 0; k < dataDim; k++)
			stdDerivation(j,k) = 1.0/sqrt(stdDerivations[j]*stdDerivations[k]);
	for (int j = 0; j < dataDim; j++)
		for (int k = 0; k < dataDim; k++)
			covMatrix(j,k) = covMatrix(j,k)*stdDerivation(j,k);

    delete[] stdDerivations;
	return covMatrix;
}

matrix CMatrixOperation::getEigenValues(matrix inputMatrix, bool sortDescening)
{
	gsl_matrix *input = inputMatrix.gslobj();
	gsl_vector *eigenValues = gsl_vector_alloc (input->size2);
	gsl_matrix *eigenVectors = gsl_matrix_alloc (input->size1, input->size2);
	gsl_eigen_symmv_workspace * w =	gsl_eigen_symmv_alloc (input->size2);
	gsl_eigen_symmv (input, eigenValues, eigenVectors, w);
	gsl_eigen_symmv_free (w);
	if (sortDescening)
		gsl_eigen_symmv_sort (eigenValues, eigenVectors,
					GSL_EIGEN_SORT_ABS_DESC);
	else
		gsl_eigen_symmv_sort (eigenValues, eigenVectors,
			GSL_EIGEN_SORT_ABS_ASC);
	matrix output(eigenVectors->size1+1, eigenVectors->size2);
    for (unsigned int i = 1; i < eigenVectors->size1+1; i++)
        for (unsigned int j = 0; j < eigenVectors->size2; j++)
			output(i,j) = gsl_matrix_get(eigenVectors, i-1, j);
    for (unsigned int i = 0; i < eigenValues->size; i++)
		output(0,i) = gsl_vector_get(eigenValues, i); //change order
    gsl_vector_free (eigenValues);
    gsl_matrix_free (eigenVectors);
    return output;
}

gsl::vector CMatrixOperation::postMultiply(matrix* dataMatrix, gsl::vector* inputVector)
{
	//mean of all dim
	gsl::vector result(inputVector->size());
	gsl_blas_dgemv(CblasNoTrans,1.0, dataMatrix->gslobj(), inputVector->gslobj(), 0.0, result.gslobj());
	return result;
}

gsl::vector CMatrixOperation::preMultiply(gsl::vector* inputVector, matrix* dataMatrix)
{
	//mean of all dim
	gsl::vector result(dataMatrix->get_cols());
	result.set_zero();
    for (unsigned int i = 0; i < dataMatrix->get_cols(); i++) {
        for (unsigned int j = 0; j < dataMatrix->get_rows(); j++) {
            result(i) += inputVector->get(j)*dataMatrix->get_element(j,i);
        }
    }
	return result;
}

bool CMatrixOperation::isOrthogonal(matrix* dataMatrix)
{
	matrix identity;
	identity.identity(dataMatrix->get_rows());
	if (*dataMatrix * dataMatrix->transpose() == identity)
		return true;
	return false;
}

bool CMatrixOperation::isSymmetric(matrix* dataMatrix)
{
    for (unsigned int i = 0; i < dataMatrix->get_rows(); i++) {
        for (unsigned int j = 0; j < dataMatrix->get_cols(); j++) {
            if (dataMatrix->get_element(i,j) != dataMatrix->get_element(j,i)) {
				return false;
            }
        }
    }
	return true;
}

bool CMatrixOperation::allEqual(matrix* dataMatrix, double value)
{
    for (unsigned int i = 0; i < dataMatrix->get_rows(); i++) {
        for (unsigned int j = 0; j < dataMatrix->get_cols(); j++) {
            if (dataMatrix->get_element(i,j) != value) {
				return false;
            }
        }
    }
	return true;
}
void CMatrixOperation::print(matrix* dataMatrix)
{
    for (unsigned int i = 0; i < dataMatrix->get_rows(); i++)
		{
            for (unsigned int j = 0; j < dataMatrix->get_cols(); j++)
				printf("%e | ",dataMatrix->get_element(i,j));
			printf("\n");
		}
}
